#include "Protocol.h"

Protocol::Protocol(SerialPort& serial_port)
    : serial_port_(serial_port)
{
}

Protocol::~Protocol()
{
}

void Protocol::sendTarget(cv::Point3f& target)
{
    Target t { target.x, target.y, target.z };
    unsigned char uart_data[16];
    uart_data[0] = 0x03;
    uart_data[1] = 0xFC;
    memcpy(uart_data + 2 * sizeof(uchar), &t, sizeof(t));
    uart_data[14] = 0xFC;
    uart_data[15] = 0x03;
    serial_port_.Write(uart_data, sizeof(uart_data));
}

McuData mcu_data = {
    EnemyColor::RED,
    State::ARMOR_STATE
};

void Protocol::receiveData()
{
    uint8_t buffer[200];
    bool init = false;
    int index = 0;
    int position = 0;
    memset(buffer, 0, sizeof(buffer));
    while (!init) {
        serial_port_.Read(buffer + 6 * index, sizeof(mcu_data) + 4);
        if (index == 0) {
            index++;
            continue;
        }
        if (index == 25) {
            index = 0;
            position = 0;
            memset(buffer, 0, sizeof(buffer));
        }
        for (int j = 0; j < 6; j++) {
            if (buffer[0 + j + position] == 0x03 && buffer[1 + j + position] == 0xFC
                && buffer[sizeof(mcu_data) + 2 + j + position] == 0xFC
                && buffer[sizeof(mcu_data) + 3 + j + position] == 0x03) {
                memcpy(&mcu_data, buffer + 2 + j + position, sizeof(mcu_data));
                printf("%d,%d\n", buffer[2 + j + position], buffer[3 + j + position]);
                printf("receive the serial date\n");
                index = 0;
                memset(buffer, 0, sizeof(buffer));
#ifdef Windows
                Sleep(1);
#elif defined Linux
                sleep(1);
#endif             
                break;
            }
            if (j == 5) {
                index++;
                position = (index - 1) * 6;
            }
        }

    }

}
